package com.hitqz.robot.dispatch.netty.service.impl;

import cn.hutool.core.util.StrUtil;
import cn.hutool.extra.spring.SpringUtil;
import com.baomidou.mybatisplus.core.conditions.query.LambdaQueryWrapper;
import com.baomidou.mybatisplus.core.toolkit.Wrappers;
import com.google.common.cache.Cache;
import com.google.common.collect.Lists;
import com.google.common.collect.Maps;
import com.hitqz.robot.api.business.dto.RobotClientSummaryDto;
import com.hitqz.robot.api.business.entity.DispatchResourceEntity;
import com.hitqz.robot.api.business.entity.DispatchResourceRecordEntity;
import com.hitqz.robot.api.business.entity.RobotEntity;
import com.hitqz.robot.api.business.entity.RobotGroupEntity;
import com.hitqz.robot.api.dispatch.protocol.dto.RobotApplyPathDto;
import com.hitqz.robot.api.dispatch.protocol.dto.RobotCmdNavDto;
import com.hitqz.robot.api.dispatch.protocol.dto.RobotMapPosDto;
import com.hitqz.robot.biz.service.*;
import com.hitqz.robot.dispatch.netty.strategy.AlgorithmStrategy;
import com.hitqz.robot.dispatch.netty.util.CheZhouAgvUtil;
import lombok.RequiredArgsConstructor;
import lombok.extern.slf4j.Slf4j;
import org.locationtech.jts.geom.Polygon;
import org.springframework.stereotype.Service;

import java.time.LocalDateTime;
import java.util.*;
import java.util.concurrent.*;
import java.util.concurrent.locks.ReentrantLock;
import java.util.stream.Collectors;

import static com.hitqz.robot.dispatch.netty.util.CheZhouAgvUtil.clearRobot;
import static com.hitqz.robot.dispatch.netty.util.CheZhouAgvUtil.goFinalPos;

/**
 * @author xupkun
 * @date 2025/4/14
 */
@Service("cheZhouAgvStrategy")
@RequiredArgsConstructor
@Slf4j
public class CheZhouAgvService implements AlgorithmStrategy {

    private final DispatchResourceService dispatchResourceService;

    private final DispatchResourceRecordService dispatchResourceRecordService;

    private final DispatchRecordService dispatchRecordService;

    private final RobotMapPosService robotMapPosService;

    private final RobotService robotService;

    private final Map<String, String> lastApplyPos = new ConcurrentHashMap<>();

    private final Map<String, Boolean> sendMiddle = new ConcurrentHashMap<>();
    /**
     *
     */
    private final Object lockC = new Object();

    /**
     *
     */
    private final Object lockB = new Object();

    private static final Map<String, String> middlePosMap = Maps.newHashMap();

    /**
     *
     */
    private final Object lockA = new Object();

    private final Object lockD = new Object();

    private final Object lockF = new Object();


    private final Map<String, ReentrantLock> lockMap = new ConcurrentHashMap<>();

    private ReentrantLock getLock(String robotSn) {
        return lockMap.computeIfAbsent(robotSn, k -> new ReentrantLock());
    }

    private final Map<String, String> lastApplyStation = new ConcurrentHashMap<>();

    private static final ThreadPoolExecutor threadPoolExecutor = new ThreadPoolExecutor(
            200,
            200,
            60,
            TimeUnit.SECONDS,
            new LinkedBlockingQueue<Runnable>(200),
            Executors.defaultThreadFactory(),
            new ThreadPoolExecutor.DiscardOldestPolicy());


    @Override
    public void changePath(RobotGroupEntity robotGroup, RobotApplyPathDto pathDto) {
        sendMiddle.put(pathDto.getClientId(), false);
        if (pathDto.getPathList().isEmpty()) {
            CheZhouAgvUtil.clearRobot(pathDto.getClientId());
            return ;
        }
        //pathDto.getPathList().remove(0);
        // 去掉自身
        log.info("提交路径前往：{}---{}", pathDto.getClientId(), pathDto.getPathList().stream().map(RobotMapPosDto::getCode).
                collect(Collectors.joining("-")));
        CheZhouAgvUtil.sendPathFlag.put(pathDto.getClientId(), false);
        sendMiddle.put(pathDto.getClientId(), false);
        CheZhouAgvUtil.sendStopFlag.put(pathDto.getClientId(), false);
        CheZhouAgvUtil.submitPath(pathDto);
    }


    @Override
    public void calculate(int groupId, List<RobotEntity> robotEntityList) {
        Cache<String, RobotClientSummaryDto> robotCache = SpringUtil.getBean("robotCache");
        LambdaQueryWrapper<DispatchResourceEntity> wrapper = Wrappers.lambdaQuery();
        wrapper.eq(DispatchResourceEntity::getMapCode, robotEntityList.get(0).getMapCode());
        if (robotEntityList.isEmpty()) {
            return;
        }
        try {
            for (RobotEntity robotEntity : robotEntityList) {
                synchronized (robotEntity.getRobotSn().intern()) {
                    RobotApplyPathDto robotApplyPathDto = CheZhouAgvUtil.robotApplyPathDtoMap.get(robotEntity.getRobotSn());
                    RobotClientSummaryDto robotClientSummaryDto = robotCache.getIfPresent(robotEntity.getRobotSn());
                    if (robotClientSummaryDto == null) {
                        continue;
                    }
                    if (robotClientSummaryDto.getLogicOnlineStatus() == 0) {
                        continue;
                    }
                    String nearStation = robotClientSummaryDto.getPosition().getLaserPos().getNearPosCode();
                    String lastArrivePos = robotClientSummaryDto.getPosition().getLaserPos().getLastPosCode();
                    if (StrUtil.isBlank(nearStation)) {
                        nearStation = lastArrivePos;
                    }
                    LinkedList<RobotMapPosDto> currentPathList = Optional.ofNullable(robotApplyPathDto).orElse(new RobotApplyPathDto()).getPathList();
                    int pathListSize = 0;
                    if (currentPathList != null && !currentPathList.isEmpty()) {
                        pathListSize = currentPathList.size();
                    }
                    int arrivePosIndex = -1;
                    try {
                        // 如果路径列表较大，可以考虑使用哈希表加速查找
                        if (currentPathList != null && !currentPathList.isEmpty()) {
                            Map<String, Integer> pathCodeIndexMap = new HashMap<>();
                            for (int i = 0; i < pathListSize; i++) {
                                String code = currentPathList.get(i).getCode();
                                if (code != null) {
                                    pathCodeIndexMap.put(code.toUpperCase(), i); // 转换为大写以支持忽略大小写的比较
                                }
                            }
                            // 快速查找匹配的路径点位
                            if (pathCodeIndexMap.containsKey(nearStation.toUpperCase())) {
                                arrivePosIndex = pathCodeIndexMap.get(nearStation.toUpperCase());
                            }
                        }
                    } catch (NullPointerException e) {
                        log.error("解析当前位置时发生空指针异常: {}", e.getMessage(), e);
                        // 可以根据业务需求决定是否抛出异常或返回默认值
                    } catch (Exception e) {
                        log.error("解析当前位置时发生未知异常: {}", e.getMessage(), e);
                        // 根据业务需求处理其他类型的异常
                    }
                    // 计算当前点到下一个路径的距离，然后提前进行申请
                    RobotMapPosDto robotMapPosDto = null;
                    if (robotApplyPathDto != null && robotApplyPathDto.getPathList() != null) {
                        LinkedList<RobotMapPosDto> pathList = robotApplyPathDto.getPathList();
                        if (!pathList.isEmpty()) {
                            robotMapPosDto = pathList.get(0);
                            if (nearStation.equals(robotMapPosDto.getCode())) {
                                if (pathList.size() >= 2) {
                                    // 正常提前申请前两个点
                                    robotMapPosDto = pathList.get(1);
                                }else {
                                    robotMapPosDto = pathList.get(0);
                                }
                            }
                        } else {
                            // 意外出现直接去目标点
                            robotMapPosDto = new RobotMapPosDto();
                            robotMapPosDto.setPosX(robotApplyPathDto.getCodeX());
                            robotMapPosDto.setPosY(robotApplyPathDto.getCodeY());
                            robotMapPosDto.setCode(robotApplyPathDto.getCode());
                        }
                    }
                    Boolean sendMiddleFlag = sendMiddle.get(robotEntity.getRobotSn());
                    if (sendMiddleFlag == null || !sendMiddleFlag) {
                        String applyNext = applyNext(robotEntity, nearStation, robotMapPosDto);
                        processPath(robotEntity, nearStation, arrivePosIndex, currentPathList);
                        if (StrUtil.isNotBlank(applyNext) && !applyNext.equals(robotEntity.getRobotSn())) {
                            // 查询公共的路径
                            List<String> pathList = Lists.newArrayList("LM200","LM201","LM202", "LM210");
                            if(pathList.contains(nearStation) && !sendMiddle.get(robotEntity.getRobotSn())) {
                                // 上料区域附近直接藏起来到LM5
                                sendMiddle.put(robotEntity.getRobotSn(), true);
                                log.info("上料区域申请下一个区域失败直接到LM5" + robotEntity.getRobotSn());
                                RobotMapPosDto middlePos = new RobotMapPosDto();
                                middlePos.setCode("LM5");
                                goFinalPos(robotEntity.getRobotSn(), middlePos);
                                middlePosMap.put(robotEntity.getRobotSn(), "LM5");
                            }
                            if (nearStation.equals("LM29") && !sendMiddle.get(robotEntity.getRobotSn())) {
                                List<DispatchResourceEntity> areaPos = findPosInArea(robotEntity, "LM29");
                                sendMiddle.put(robotEntity.getRobotSn(), true);
                                if (!areaPos.isEmpty()) {
                                    areaPos.forEach(t -> {
                                        DispatchResourceRecordEntity entity =new DispatchResourceRecordEntity();
                                        entity.setRemark("LM29");
                                        entity.setOccupyRobot(robotEntity.getRobotSn());
                                        entity.setStatus(1);
                                        entity.setAreaId(t.getId());
                                        entity.setEndTime(LocalDateTime.now());
                                        entity.setStartTime(LocalDateTime.now());
                                        dispatchResourceRecordService.save(entity);
                                    });
                                } else {
                                    DispatchResourceRecordEntity entity =new DispatchResourceRecordEntity();
                                    entity.setRemark("LM29");
                                    entity.setOccupyRobot(robotEntity.getRobotSn());
                                    entity.setStatus(1);
                                    entity.setAreaId(0);
                                    entity.setEndTime(LocalDateTime.now());
                                    entity.setStartTime(LocalDateTime.now());
                                    dispatchResourceRecordService.save(entity);
                                }
                                RobotMapPosDto middlePos = new RobotMapPosDto();
                                middlePos.setCode("LM58");
                                goFinalPos(robotEntity.getRobotSn(), middlePos);
                                middlePosMap.put(robotEntity.getRobotSn(), "LM58");
                                // 另外一辆车锁定LM29
                                List<DispatchResourceEntity> source = findPosInArea(robotEntity, "LM29");
                                if (!source.isEmpty()) {
                                    source.forEach(t -> {
                                        DispatchResourceRecordEntity entity =new DispatchResourceRecordEntity();
                                        entity.setRemark("LM29");
                                        entity.setOccupyRobot(applyNext);
                                        entity.setStatus(1);
                                        entity.setAreaId(t.getId());
                                        entity.setEndTime(LocalDateTime.now());
                                        entity.setStartTime(LocalDateTime.now());
                                        dispatchResourceRecordService.save(entity);
                                    });
                                }
                                // 增加申请LM29
                                RobotMapPosDto robotMapPosDto1 = new RobotMapPosDto();
                                robotMapPosDto1.setCode("LM29");
                                robotMapPosDto1.setName("LM29");
                                currentPathList.addFirst(robotMapPosDto1);
                                currentPathList.addFirst(robotMapPosDto1);
                                robotApplyPathDto.setPathList(currentPathList);
                                CheZhouAgvUtil.robotApplyPathDtoMap.put(robotEntity.getRobotSn(), robotApplyPathDto);
                            }
                            if (nearStation.equals("LM58") && robotMapPosDto.getCode().equals("LM29")) {
                                RobotClientSummaryDto blockRobotStatus = robotCache.getIfPresent(applyNext);
                                if (blockRobotStatus != null) {
                                    if (blockRobotStatus.getPosition().getLaserPos().getNearPosCode().equals("LM212")) {
                                        // 如果当前点事可以的
                                        log.info("{} 阻挡的那个车到达附近的点LM28", applyNext);
                                        assert currentPathList != null;
                                        LinkedList<RobotMapPosDto> newPathList = new LinkedList<>();

                                        currentPathList.forEach(t-> {
                                            if (!t.getCode().equals("LM29")) {
                                                newPathList.addLast(t);
                                            }
                                        });
                                        robotApplyPathDto.setPathList(newPathList);
                                        CheZhouAgvUtil.robotApplyPathDtoMap.put(robotEntity.getRobotSn(), robotApplyPathDto);
                                    }else {
                                        RobotEntity robot = robotService.selectByRobotSn(applyNext);
                                        List<DispatchResourceEntity> source = findPosInArea(robot, "LM29");
                                        if (!source.isEmpty()) {
                                            source.forEach(t -> {
                                                DispatchResourceRecordEntity entity =new DispatchResourceRecordEntity();
                                                entity.setRemark("LM29");
                                                entity.setOccupyRobot(applyNext);
                                                entity.setStatus(1);
                                                entity.setAreaId(t.getId());
                                                entity.setEndTime(LocalDateTime.now());
                                                entity.setStartTime(LocalDateTime.now());
                                                dispatchResourceRecordService.save(entity);
                                            });
                                        }
                                    }
                                }
                            }
                        } else {
                            String station = lastApplyStation.get(robotEntity.getRobotSn());
                            if (StrUtil.isBlank(station) || !station.equals(nearStation)) {
                                lastApplyStation.put(robotEntity.getRobotSn(), nearStation);
                            }
                        }
                    }
                    if (sendMiddleFlag != null && sendMiddleFlag) {
                        String middle = middlePosMap.get(robotEntity.getRobotSn());
                        if (StrUtil.isNotBlank(middle) && nearStation.equals(middle) &&
                                robotClientSummaryDto.getStatus().getNavStatus() == 4) {
                            sendMiddle.put(robotEntity.getRobotSn(), false);
                            log.info("{} 前往中间点结束,继续申请点 {} ", robotEntity.getRobotSn(),
                                    robotMapPosDto != null ? robotMapPosDto.getCode() : "test默认");
                            //更新区域
                            List<DispatchResourceEntity> list = findPosInArea(robotEntity, nearStation);
                            dispatchResourceRecordService.update(
                                    Wrappers.<DispatchResourceRecordEntity>lambdaUpdate()
                                            .set(DispatchResourceRecordEntity::getStatus, 3)
                                            .eq(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
                                            .eq(DispatchResourceRecordEntity::getStatus, 1)
                            );
                            if (list.isEmpty()) {
                                DispatchResourceRecordEntity dispatchResourceRecordEntity = new DispatchResourceRecordEntity();
                                dispatchResourceRecordEntity.setRemark(nearStation);
                                dispatchResourceRecordEntity.setOccupyRobot(robotEntity.getRobotSn());
                                dispatchResourceRecordEntity.setStatus(1);
                                // 查询当前点是否在区域内
                                dispatchResourceRecordEntity.setAreaId(0);
                                dispatchResourceRecordEntity.setEndTime(LocalDateTime.now());
                                dispatchResourceRecordEntity.setStartTime(LocalDateTime.now());
                                dispatchResourceRecordService.save(dispatchResourceRecordEntity);
                            } else {
                                String finalNearStation = nearStation;
                                list.forEach(t -> {
                                    synchronized (lockD) {
                                        DispatchResourceRecordEntity entity =
                                                dispatchResourceRecordService.getOne(
                                                        Wrappers.<DispatchResourceRecordEntity>lambdaQuery()
                                                                .eq(DispatchResourceRecordEntity::getAreaId, t.getId())
                                                                .eq(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
                                                                .eq(DispatchResourceRecordEntity::getRemark, finalNearStation)
                                                                .eq(DispatchResourceRecordEntity::getStatus, 1)
                                                                .last("limit 1")
                                                );
                                        if (entity == null) {
                                            entity = new DispatchResourceRecordEntity();
                                            entity.setRemark(finalNearStation);
                                            entity.setOccupyRobot(robotEntity.getRobotSn());
                                            entity.setStatus(1);
                                            entity.setAreaId(t.getId());
                                            entity.setEndTime(LocalDateTime.now());
                                            entity.setStartTime(LocalDateTime.now());
                                            dispatchResourceRecordService.save(entity);
                                        }
                                    }
                                });
                            }
                            TimeUnit.SECONDS.sleep(3);
                        }
                    }
                    // 删除多余的数据
                    dispatchResourceRecordService.remove(Wrappers.<DispatchResourceRecordEntity>lambdaUpdate()
                            .eq(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
                            .eq(DispatchResourceRecordEntity::getStatus, 3)
                    );
                }
            }

        } catch (Exception e) {
            log.error("获取资源信息失败", e);
        }

    }

    /**
     * 对路径数据进行处理
     */
    public void processPath(RobotEntity robotEntity, String nearStation, int arrivePosIndex, LinkedList<RobotMapPosDto> currentPathList) {
        lockC(lockC, () -> {
            if (arrivePosIndex == 0 && !currentPathList.isEmpty()) {
                logArrivalInfo(nearStation, currentPathList, arrivePosIndex, robotEntity);
                //updateDispatchResourceRecords(robotEntity);
            }
        });
    }

    private void lockC(Object lock, Runnable task) {
        synchronized (lock) {
            try {
                task.run();
            } catch (Exception e) {
                log.error("路径处理过程中发生异常: {}", e.getMessage(), e);
            }
        }
    }

    private void logArrivalInfo(String nearStation, LinkedList<RobotMapPosDto> currentPathList, int arrivePosIndex, RobotEntity robotEntity) {
        String pathCodes = currentPathList.stream()
                .map(RobotMapPosDto::getCode)
                .collect(Collectors.joining("-"));
        //log.info("到达点位是 {} 计算之前的路径: {}", nearStation, pathCodes);
        RobotApplyPathDto robotApplyPathDto = CheZhouAgvUtil.robotApplyPathDtoMap.get(robotEntity.getRobotSn());
        RobotMapPosDto robotMapPosDto = currentPathList.get(arrivePosIndex);
        if (nearStation.equals(robotMapPosDto.getCode())) {
            currentPathList.remove(arrivePosIndex);
            robotApplyPathDto.setPathList(currentPathList);
            CheZhouAgvUtil.robotApplyPathDtoMap.put(robotEntity.getRobotSn(), robotApplyPathDto);
            String remainingPathCodes = robotApplyPathDto.getPathList().stream()
                    .map(RobotMapPosDto::getCode)
                    .collect(Collectors.joining("-"));
            //log.info("AGV执行剩余路径22222: {}", remainingPathCodes);
            if (currentPathList.isEmpty()) {
                CheZhouAgvUtil.clearRobot(robotEntity.getRobotSn());
            }
        }

    }




    private void send2RobotNav(RobotEntity robotEntity, RobotMapPosDto dto) {
        // 参数校验
        try {
            // 检查是否需要发送路径
            if (!CheZhouAgvUtil.sendPathFlag.computeIfAbsent(robotEntity.getRobotSn(), key -> false)) {
                CheZhouAgvUtil.sendPathFlag.put(robotEntity.getRobotSn(), true); // 标记为已发送
                //log.info("机器人222222 {} 下发可以前往位置 {}", robotEntity.getRobotSn(), dto.getCode());
                // 写入区域被占用
                // 调用外部方法发送最终位置
                log.info("申请通过: {}-{}", robotEntity.getRobotSn(), dto.getCode());
                goFinalPos(robotEntity.getRobotSn(), dto);
            }
        } catch (Exception e) {
            log.error("发送路径失败，机器人编号: {}, 目标位置: {}", robotEntity.getRobotSn(), dto.getCode(), e);
        }
    }


    /**
     * 修改导航状态
     *
     * @param status 导航状态
     */
    public static void changeNavStatus(RobotEntity robotEntity, int status, RobotMapPosDto robotMapPosDto) {
        // 确保 clientId 不为空
        // 获取 sendPathFlag 的值并避免多次调用 get 方法
        Boolean sendPathFlagValue = CheZhouAgvUtil.sendPathFlag.get(robotEntity.getRobotSn());
        if (sendPathFlagValue != null && sendPathFlagValue) {
            try {
                Cache<String, RobotClientSummaryDto> robotCache = SpringUtil.getBean("robotCache");
                if (robotCache != null) {
                    RobotClientSummaryDto robotClientSummaryDto = robotCache.getIfPresent(robotEntity.getRobotSn());
                    assert robotClientSummaryDto != null;
                    int taskStatus = robotClientSummaryDto.getStatus().getNavStatus();
                    if (taskStatus == 2 && status == 1) {
                        status = 3;
                        // 取消导航
                        SpringUtil.getApplicationContext().publishEvent(new RobotCmdNavDto(robotEntity.getRobotSn(), status));
                        TimeUnit.SECONDS.sleep(1);
                    }
                    if ((taskStatus == 4 ||taskStatus == 6) && status == 2) {
                        RobotApplyPathDto robotApplyPathDto = CheZhouAgvUtil.robotApplyPathDtoMap.get(robotEntity.getRobotSn());
                        if (robotApplyPathDto != null && robotApplyPathDto.getCode() != null) {
                            String nearStation = robotClientSummaryDto.getPosition().getLaserPos().getNearPosCode();
                            if (!nearStation.equals(robotApplyPathDto.getCode())) {
                                RobotMapPosDto finalPos = new RobotMapPosDto();
                                finalPos.setClientId(robotEntity.getRobotSn());
                                finalPos.setCode(robotApplyPathDto.getCode());
                                goFinalPos(robotEntity.getRobotSn(), finalPos);
                            } else {
                                clearRobot(robotEntity.getRobotSn());
                            }
                        }
                        TimeUnit.SECONDS.sleep(1);
                    }
                }
                if (status == 1) {
                    CheZhouAgvUtil.sendStopFlag.put(robotEntity.getRobotSn(), true);
                }
                if (status == 2) {
                    CheZhouAgvUtil.sendStopFlag.put(robotEntity.getRobotSn(), false);
                }
            } catch (Exception e) {
                // 捕获异常并记录详细日志
                log.error("发布导航状态事件失败，clientId: {}, status: {}", robotEntity.getRobotSn(), status, e);
            }
        }
    }


    public String applyNext(RobotEntity robotEntity, String nearStation, RobotMapPosDto robotMapPosDto) {
        Cache<String, Polygon> robotPosPolygon = SpringUtil.getBean("robotPosPolygon");
        Polygon polygon = robotPosPolygon.getIfPresent(robotEntity.getRobotSn());
        if (polygon == null) {
            return "";
        }
        // 如果下一个点是null 表示当前是不动的
        String pointCode;
        if (robotMapPosDto != null) {
            pointCode = robotMapPosDto.getCode();
        } else {
            pointCode = nearStation;
        }
        if (nearStation.equals("LM5") && pointCode.equals("LM200") ||
                nearStation.equals("LM200") && pointCode.equals("LM201") ||
                nearStation.equals("LM201") && pointCode.equals("LM202")||
                nearStation.equals("LM200") && pointCode.equals("LM202") ||
                nearStation.equals("LM5") && pointCode.equals("LM202")
        ) {
            // 申请出来那个点
            pointCode = "LM29";
        }
        synchronized (lockB) {
            if (robotMapPosDto == null) {
                // 没有下一个目标点
                // 当前车是不动的写入当前被占用
                DispatchResourceRecordEntity dispatchResourceRecordEntity = dispatchResourceRecordService.getOne(
                        Wrappers.<DispatchResourceRecordEntity>lambdaQuery()
                                .eq(DispatchResourceRecordEntity::getAreaId, 0)
                                .eq(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
                                .eq(DispatchResourceRecordEntity::getRemark, pointCode)
                                .eq(DispatchResourceRecordEntity::getStatus, 1)
                                .last("limit 1")
                );
                if (dispatchResourceRecordEntity == null) {
                    List<DispatchResourceEntity> list = findPosInArea(robotEntity, nearStation);
                    if (list.isEmpty()) {
                        dispatchResourceRecordEntity = new DispatchResourceRecordEntity();
                        dispatchResourceRecordEntity.setRemark(pointCode);
                        dispatchResourceRecordEntity.setOccupyRobot(robotEntity.getRobotSn());
                        dispatchResourceRecordEntity.setStatus(1);
                        // 查询当前点是否在区域内
                        dispatchResourceRecordEntity.setAreaId(0);
                        dispatchResourceRecordEntity.setEndTime(LocalDateTime.now());
                        dispatchResourceRecordEntity.setStartTime(LocalDateTime.now());
                        dispatchResourceRecordService.save(dispatchResourceRecordEntity);
                    } else {
                        String finalPointCode = pointCode;
                        list.forEach(t -> {
                            synchronized (lockD) {
                                DispatchResourceRecordEntity entity =
                                        dispatchResourceRecordService.getOne(
                                                Wrappers.<DispatchResourceRecordEntity>lambdaQuery()
                                                        .eq(DispatchResourceRecordEntity::getAreaId, t.getId())
                                                        .eq(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
                                                        .eq(DispatchResourceRecordEntity::getRemark, finalPointCode)
                                                        .eq(DispatchResourceRecordEntity::getStatus, 1)
                                                        .last("limit 1")
                                        );
                                if (entity == null) {
                                    entity = new DispatchResourceRecordEntity();
                                    entity.setRemark(finalPointCode);
                                    entity.setOccupyRobot(robotEntity.getRobotSn());
                                    entity.setStatus(1);
                                    // 查询当前点是否在区域内
                                    entity.setAreaId(t.getId());
                                    entity.setEndTime(LocalDateTime.now());
                                    entity.setStartTime(LocalDateTime.now());
                                    dispatchResourceRecordService.save(entity);
                                }
                            }
                        });
                    }
                }
                dispatchResourceRecordService.update(
                        Wrappers.<DispatchResourceRecordEntity>lambdaUpdate()
                                .set(DispatchResourceRecordEntity::getStatus, 3)
                                .ne(DispatchResourceRecordEntity::getRemark, pointCode)
                                .eq(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
                                .eq(DispatchResourceRecordEntity::getStatus, 1)
                                .last("limit 1")
                );
            } else {
                if (CheZhouAgvUtil.sendPathFlag.get(robotEntity.getRobotSn()) != null && !CheZhouAgvUtil.sendPathFlag.get(robotEntity.getRobotSn())) {
                    // 没有下发过导航
                    String applyArea = applyArea(robotEntity, pointCode);
                    //log.info("{} agv申请下一个点 {}结果", robotEntity.getIpAddress(), pointCode, applyArea);
                    if (StrUtil.isEmpty(applyArea)) {
                        RobotApplyPathDto robotApplyPathDto = CheZhouAgvUtil.robotApplyPathDtoMap.get(robotEntity.getRobotSn());
                        if (robotApplyPathDto != null) {
                            if (robotApplyPathDto.getPathList().isEmpty()) {
                                return "";
                            }
                            RobotMapPosDto finalPos = new RobotMapPosDto();
                            finalPos.setCode(robotApplyPathDto.getCode());
                            log.info("申请1111111---{}---{}", robotEntity.getRobotSn(), robotApplyPathDto.getCode());
                            send2RobotNav(robotEntity, finalPos);
                        }
                    }
                    return applyArea.equals(robotEntity.getRobotSn()) ? "": applyArea;
                } else {
                    // 下发过
                    String applyResult = applyArea(robotEntity, pointCode);
                    if (StrUtil.isNotEmpty(applyResult)) {
                        // 不通过
                        changeNavStatus(robotEntity, 1, robotMapPosDto);
                        return applyResult.equals(robotEntity.getRobotSn()) ? "": applyResult;
                    } else {
                        changeNavStatus(robotEntity, 2, robotMapPosDto);
                    }
                }

            }
        }
        return "";
    }


    /**
     * 申请区域
     *
     * @param robotEntity
     * @param nextPointCode
     * @return
     */
    private String applyArea(RobotEntity robotEntity, String nextPointCode) {
        ReentrantLock lock = getLock(robotEntity.getRobotSn());
        lock.lock();
        String writeNext = "";
        try {
            // 查询出来下一个点所在的区域
            List<DispatchResourceEntity> prepareArea = findPosInArea(robotEntity, nextPointCode);
            if (prepareArea.isEmpty()) {
                DispatchResourceRecordEntity dispatchResourceRecordEntity = dispatchResourceRecordService.getOne(
                        Wrappers.<DispatchResourceRecordEntity>lambdaQuery()
                                .eq(DispatchResourceRecordEntity::getAreaId, 0)
                                .eq(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
                                .eq(DispatchResourceRecordEntity::getRemark, nextPointCode)
                                .eq(DispatchResourceRecordEntity::getStatus, 1)
                                .last("limit 1")
                );
                if (dispatchResourceRecordEntity == null) {
                    dispatchResourceRecordEntity = new DispatchResourceRecordEntity();
                    dispatchResourceRecordEntity.setRemark(nextPointCode);
                    dispatchResourceRecordEntity.setOccupyRobot(robotEntity.getRobotSn());
                    dispatchResourceRecordEntity.setStatus(1);
                    // 查询当前点是否在区域内
                    dispatchResourceRecordEntity.setAreaId(0);
                    dispatchResourceRecordEntity.setEndTime(LocalDateTime.now());
                    dispatchResourceRecordEntity.setStartTime(LocalDateTime.now());
                    dispatchResourceRecordService.save(dispatchResourceRecordEntity);
                }
                dispatchResourceRecordService.update(
                        Wrappers.<DispatchResourceRecordEntity>lambdaUpdate()
                                .set(DispatchResourceRecordEntity::getStatus, 3)
                                .ne(DispatchResourceRecordEntity::getRemark, nextPointCode)
                                .eq(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
                                .eq(DispatchResourceRecordEntity::getStatus, 1)
                );
            } else {
                // 查询准备申请的区域是否已经被占用
                List<DispatchResourceRecordEntity> existingArea = dispatchResourceRecordService.list(
                        Wrappers.<DispatchResourceRecordEntity>lambdaQuery()
                                .ne(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
                                .eq(DispatchResourceRecordEntity::getStatus, 1)
                                .in(DispatchResourceRecordEntity::getAreaId,
                                        prepareArea.stream().map(DispatchResourceEntity::getId).toList())
                );
                if (existingArea != null && !existingArea.isEmpty()) {
                    return existingArea.get(0).getOccupyRobot();
                }
                // 写入准备申请的区域
                prepareArea.forEach(t -> {
                    DispatchResourceRecordEntity entity = dispatchResourceRecordService.getOne(
                            Wrappers.<DispatchResourceRecordEntity>lambdaQuery()
                                    .eq(DispatchResourceRecordEntity::getAreaId,
                                            t.getId())
                                    .eq(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
                                    .eq(DispatchResourceRecordEntity::getStatus, 1));

                    if (entity == null) {
                        entity = new DispatchResourceRecordEntity();
                        entity.setRemark(nextPointCode);
                        entity.setOccupyRobot(robotEntity.getRobotSn());
                        entity.setStatus(1);
                        // 查询当前点是否在区域内
                        entity.setAreaId(t.getId());
                        entity.setEndTime(LocalDateTime.now());
                        entity.setStartTime(LocalDateTime.now());
                        dispatchResourceRecordService.save(entity);
                    }
                });
                // 查询是否存在当前的
                dispatchResourceRecordService.update(
                        Wrappers.<DispatchResourceRecordEntity>lambdaUpdate()
                                .set(DispatchResourceRecordEntity::getStatus, 3)
                                .notIn(DispatchResourceRecordEntity::getAreaId,
                                        prepareArea.stream().map(DispatchResourceEntity::getId).toList())
                                .eq(DispatchResourceRecordEntity::getOccupyRobot, robotEntity.getRobotSn())
                                .eq(DispatchResourceRecordEntity::getStatus, 1)
                );
            }
        } catch (Exception e) {

        } finally {
            lock.unlock();
        }
        return writeNext;
    }


    /**
     * 查询申请的点位是不是在区域内
     *
     * @param robotEntity
     * @param posCode
     * @return
     */
    private List<DispatchResourceEntity> findPosInArea(RobotEntity robotEntity, String posCode) {
        List<DispatchResourceEntity> allResource = dispatchResourceService.list(
                Wrappers.<DispatchResourceEntity>lambdaQuery()
                        .eq(DispatchResourceEntity::getMapCode, robotEntity.getMapCode())
        );
        return allResource.stream().filter(item -> {
            String[] path = item.getPathList().split(",");
            for (String s : path) {
                if (s.equals(posCode)) {
                    return true;
                }
            }
            return false;
        }).toList();
    }


}
